#ifndef ZEUS_S3_ODOM_HPP
#define ZEUS_S3_ODOM_HPP

#include <cstdint>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <tf2/LinearMath/Quaternion.h>
#include <string>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include "ascent/Ascent.h"
#include "ascent/Utility.h"
#include "zeus_s3_odom/bicycle_model.hpp"
#include <sensor_msgs/JointState.h>
#include "zeus_s3_msgs/Feedback.h"
#include "zeus_s3_msgs/wheel_state.h"

#define MAX_LINEAR  1.36        //m/s
#define MAX_ANGULAR  0.39       //red/s
#define WHEEL_BASE  0.6         //轮距
#define GROUP_INT_BYTE(X,N)   ((X[N+3]<<24)+(X[N+2]<<16)+(X[N+1]<<8)+X[N])
#define  MOTOR_ID(X,N)   ((X[N]<<8 | X[N+1])>>5)

template <typename SystemModel>
class SystemPropagator {
 public:
  asc::state_t Propagate(asc::state_t init_state,
                         typename SystemModel::control_t u, double t0,
                         double tf, double dt) {
    double t = t0;
    asc::state_t x = init_state;

    while (t <= tf) {
      integrator_(SystemModel(u), x, t, dt);
      // Note: you may need to add additional constraints to [x]
    }

    return x;
  }

 private:
  asc::RK4 integrator_;
};

class DriverOdomPub
{
   public:
        DriverOdomPub();
        ~DriverOdomPub();
        void loop();

    private:

        void handle_speed_data(void);
        void motor_feedback_callback( const zeus_s3_msgs::Feedback::ConstPtr& msg);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void ResetOdomIntegratorCallback(const std_msgs::Bool::ConstPtr &msg);
        void ResetOdometry();
        void PublishOdometryToROS(double linear, double angular, double dt);

        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
        ros::Subscriber motor_driver_sub_;
        ros::Subscriber integrator_reset_subscriber_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_join_;
        zeus_s3_msgs::Feedback  left_right_wheel_state_;
        ros::Publisher speed_pid_;              //发布测试pid话题

        SystemPropagator<BicycleKinematics> model_;
        

        std::string port_name_;

        std::string odom_frame_;
        std::string base_frame_;

        bool recv_flag_;
        bool start_flag_;
        bool publish_odom_transform_;
        zeus_s3_msgs::Feedback current_speed_;
        geometry_msgs::Twist  current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
        tf2_ros::TransformBroadcaster br_;
        
        int fdcom;
        int baud_rate_;
        unsigned char buffer_data[100];

        int ret;
        ros::Time now_;
        ros::Time last_time_;

        double wheel_track_4wd_;                      //轮距
        double wheelbase_;                            //轴距
        double wheel_radius_;                         //轮子半径
        double encoder_resolution_4wd_;
        double ticks_per_meter_;       //脉冲数/m
        double max_steer_angle_;       //内最大转角
        double max_steer_angle_central_;//最大中心转角
        double max_linear_speed_;       //最大线速度

        double linear_correction_factor_;
        double angular_correction_factor_;
        double gear_reduction_4wd_;     //减速比
        double wheel_diameter_4wd_;    //直径
        double start_rotation_limit_w_;
        double delta_time_;
        double delta_xy_ave_;
        double delta_th_;
        double v_linear_;
        double v_angular_;
        double delta_x_;
        double delta_y_;

        double accumulation_x_;
        double accumulation_y_;
        double accumulation_th_;
        double accumulation_left_;           //记录轮子转动角度
        double accumulation_left_1_;
        double accumulation_right_;
        double accumulation_right_1_;

        double delta_left_;
        double delta_right_;        //里程差值 单位 m
        double delta_left_1_;
        double delta_right_1_;        //里程差值 单位 m
        int32_t rev_left_;
        int32_t rev_left_1_;
        int32_t rev_right_;       
        int32_t rev_right_1_;
        int cur_left_;              //存放脉冲计数值
        int cur_left_1_; 
        int cur_right_;
        int cur_right_1_;
        double encoder_low_wrap_;       //解决编码器溢出
        double encoder_high_wrap_;
        int l_wheel_mult_;              //多圈计数
        int l_wheel_mult_1_;              //多圈计数
        int r_wheel_mult_;
        int r_wheel_mult_1_;
        int encoder_min_;
        int encoder_max_;   

        int32_t line_speed_percent;
        int32_t angle_speed_percent;

        uint8_t queueFlag;
        uint8_t can_recv_flag;

        float motor_kp;
        float motor_ki;
        float motor_kd;
        int ele_currVol;
        float speed_l;
        float speed_r;
        short error;


        int16_t  motor_ctlVol_one;
        int16_t  motor_ctlVol_two;
        int16_t  motor_ctlVol_three;
        int16_t  motor_ctlVol_four;
        short left_des;
        short right_des;
        bool  reset_vol;
        bool  cmd_vel_flag;
        bool  rev_speed_flag;

        // control inputs
        double linear_speed_ = 0.0;
        double steering_angle_ = 0.0;
        // state variables
        double position_x_ = 0.0;
        double position_y_ = 0.0;
        double theta_ = 0.0;

};



#endif /* ZEUS_S3_ODOM_HPP */